# -*- coding: utf-8 -*-
from naoqi import *
import time
import math
import time
import jieguo as jie


from naoqi import ALProxy



robotIP="192.168.1.5"
PORT=9559


Config = [["MaxStepX", 0.06],
           ["MaxStepY", 0.13],
           ["MaxStepTheta", 0.4],
           ["MaxStepFrequency", 0.7],
           ["StepHeight", 0.03],
           ["TorsoWx", 0.0],
           ["TorsoWy", 0.0]]
motion = ALProxy("ALMotion", robotIP, PORT)
posture = ALProxy("ALRobotPosture", robotIP, PORT)
#motionProxy = ALProxy("ALMotion", robotIP, PORT)
#postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
#sonarProxy = ALProxy("ALSonar", robotIP, PORT)
#memoryProxy = ALProxy("ALMemory", robotIP, PORT)
#sonarProxy.subscribe("myApplication")

#motionProxy.wakeUp()
motion.wakeUp()
posture.goToPosture("StandInit", 0.5)
#postureProxy.goToPosture("StandInit", 0.5)



#直走到指定位置开始识别第一个
#motionProxy.moveTo(0.5, 0, 0)
motion.move(0.5, 0, 0, Config)  # 起步
time.sleep(3)

a1=jie.test()
print a1


if(a1==True ):
        # t=1*math.pi/20

        # motionProxy.moveTo(1.5, y, t)
        #motionProxy.moveTo(0.5, 0, 0)
        '''motionProxy.moveTo(0, -0.85, 0)
        motionProxy.moveTo(1.0, 0, 0)
        motionProxy.moveTo(0, 0.7, 0)
        time.sleep(1)'''
        t = 1 * math.pi / 2
        motion.move(0.5, 0, t, Config)
        time.sleep(3)
        motion.move(0, 0, -t, Config)
        print "a1=true"

    # 锁定头部，让其低头
    #ALM.angleInterpolation("HeadYaw", -0.00, 1, False)

    #ALM.angleInterpolation("HeadPitch", 0.5, 1, False)
    #ALM.angleInterpolation('HeadYaw', -30 * almath.TO_RAD, 0.2)

    # 头部左转30°

elif(a1==None):
    #if(a1==False):zuo
    t = 1 * math.pi / 2
    motion.move(0.5, 0, -t, Config)  # 一弯
    time.sleep(3)
    print(2)
    motion.move(0, 0, t, Config)  # 一弯
    # time.sleep(1.5)
    # motionProxy.moveTo(0.5, 0, 0)
    '''motionProxy.moveTo(0, 0.6, 0)
    # motionProxy.move(1.7, 0, 0)
    motionProxy.moveTo(0.9, 0, 0)
    motionProxy.moveTo(0, 0, -t)
    motionProxy.moveTo(0, -0.65, 0)
    print "a1=false"
    time.sleep(1)'''



#开始识别第二个
a2=jie.test()
print a2

if(a2==True ):
        t = 1 * math.pi / 2
        motion.move(0.5, 0, t, Config)
        time.sleep(3)
        motion.move(0, 0, -t, Config)
        # motionProxy.moveTo(1.5, y, t)
        #motionProxy.moveTo(0.5, 0, 0)
        '''motionProxy.moveTo(0, -0.4, 0)
        motionProxy.moveTo(0, -0.4, -t)
        motionProxy.moveTo(0.8, 0, 0)
        motionProxy.moveTo(0, 0.7, 0)'''
        print "a2=true"

        #i1=0


elif (a2 == None):

    # time.sleep(1.5)
    # motionProxy.moveTo(0.5, 0, 0)
    '''motionProxy.moveTo(0, 0.6, 0)
    motionProxy.moveTo(0.9, 0, 0)
    motionProxy.moveTo(0, -0.6, 0)'''
    t = 1 * math.pi / 2
    motion.move(0.5, 0, -t, Config)  # 一弯
    time.sleep(3)
    motion.move(0, 0, t, Config)
    print "a2=false"
 #   time.sleep(1)
    #i1 = 1


#开始识别第三个
time.sleep(1)
a3=jie.test()
print a3

if(a3==True):
    '''motionProxy.moveTo(0, -0.7, 0)
    motionProxy.moveTo(0.9, 0, 0)
    motionProxy.moveTo(0, 0.7, 0)'''
    t = 1 * math.pi / 2
    motion.move(0.5, 0, t, Config)
    time.sleep(3)
    motion.move(0, 0, -t, Config)
    motion.moveTo(0.8,0,0)
    print "a3=True"
elif(a3==None):
    '''motionProxy.moveTo(0, 0.8, 0)
    motionProxy.moveTo(1, 0, 0)
    motionProxy.moveTo(0, -0.6, 0)'''
    t = 1 * math.pi / 2
    motion.move(0.5, 0, -t, Config)  # 一弯
    time.sleep(3)
    print(2)
    motion.move(0, 0, t, Config)
    print "a3=false"
    motion.moveTo(0.8, 0, 0, Config)

'''if(i1==0 and a2 ==jie.test()):
    if(a2==True):
        y=0
        t =0
        i2=0
        motionProxy.moveTo(1, y, t)


elif(i1==1 and a2==jie.test()):
    if(a2==True):
        motionProxy.moveTo(0, 0.46, 0)
        # t=1*math.pi/20
        time.sleep(3)
        # motionProxy.moveTo(1.5, y, t)
        motionProxy.move(1.5, 0, 0)
        while 1:
            if (bizhang() == 1):
                motionProxy.moveTo(0, 0.3, 0)
                motionProxy.moveTo(0.6, 0, 0)
                time.sleep(0.1)
                break

        # time.sleep(0.3)
        # motionProxy.moveTo(0, -0.3, 0)
        motionProxy.move(1.5, 0, 0)
        i2 = 0
        print "a2=true"



elif(i1==0 and a2 == jie.test()):
    if(a2==False):
        motionProxy.moveTo(0, -0.46, 0)
        # t=1*math.pi/20
        time.sleep(3)
        # motionProxy.moveTo(1.5, y, t)
        motionProxy.move(1.5, 0, 0)
        while 1:
            b1=0
            if (bizhang() == 1):
                motionProxy.moveTo(0, -0.3, 0)
                motionProxy.moveTo(0.6, 0, 0)
                time.sleep(0.1)
                b1=1
                break
            if(b1==0):
                motionProxy.move(1.5,0,0)
                break
        #time.sleep(0.3)
        #motionProxy.moveTo(0, -0.3, 0)
        motionProxy.move(1.5, 0, 0)
        i2=1
        print "a2=false"



elif(i1 == 1 and a2 == jie.test()):
    if(a2==False):
        y = 0
        t = 0
        i2 = 1
        motionProxy.moveTo(1, y, t)


if(i2==0 and a3 == jie.test()):
    if(a3==True):
        y=0
        t = 0
        i3=0
        motionProxy.moveTo(1, y, t)


elif(i2==1 and a3 == jie.test()):
    if(a3==True):
        motionProxy.moveTo(0, 0.6, 0)
        # t=1*math.pi/20
        time.sleep(2)
        # motionProxy.moveTo(1.5, y, t)
        motionProxy.move(1, 0, 0)
        while 1:
            if (bizhang() == 1):
                motionProxy.moveTo(0, 0.3, 0)
                motionProxy.moveTo(1, 0, 0)
                time.sleep(0.1)
                break

        #time.sleep(0.3)
        #motionProxy.moveTo(0, 0.3, 0)
        motionProxy.move(1, 0, 0)
        print "a3=true"
    #ALM.angleInterpolation("HeadYaw", -0.00, 1, False)

        #ALM.angleInterpolation('HeadYaw', 30 * almath.TO_RAD, 0.2)
elif(i2==0 and a3 == jie.test()):
    if(a3==False):
        motionProxy.moveTo(0, -0.46, 0)
        # t=1*math.pi/20
        time.sleep(3)
        # motionProxy.moveTo(1.5, y, t)
        motionProxy.move(1.5, 0, 0)
        while 1:
            if (bizhang() == 1):
                motionProxy.moveTo(0, -0.3, 0)
                motionProxy.moveTo(0.6, 0, 0)
                time.sleep(0.1)
                break

        # time.sleep(0.3)
        # motionProxy.moveTo(0, -0.3, 0)
        motionProxy.move(1.5, 0, 0)

        print "a3=false"




elif(i2 == 1 and a3 == jie.test()):
    if(a3==False):
        y = 0
        t = 0
        i3 = 1
        motionProxy.moveTo(1, y, t)'''


